#include <Arduino.h>
#include "hexapod.h"
#include "servo.h"
//#include "debug.h"

// TBD: move to hal
#include <EEPROM.h>
#define EEPROM_SIZE 72

void Flash_write( const uint8_t *data,  uint8_t size)
{
unsigned char i;
int addr = 0;

if (!EEPROM.begin(EEPROM_SIZE))
{
  Serial.println("failed to initialise EEPROM"); delay(1000000);
}

for(i=0;i<size;i++){

EEPROM.write(addr+i, data[i]);

}
 EEPROM.commit();
}





 void Flash_read( uint8_t *data,  uint8_t size)
{
		
		const int addr = 0;

		if (!EEPROM.begin(EEPROM_SIZE))
		{
		  Serial.println("failed to initialise EEPROM"); delay(1000000);
		}
   
		for(int i=0;i<size;i++){

	  data[i]=byte(EEPROM.read(i));

		}
}

namespace hexapod {

    HexapodClass Hexapod;

    HexapodClass::HexapodClass(): 
        legs_{{0}, {1}, {2}, {3}, {4}, {5}}, 
        movement_{MOVEMENT_STANDBY},
        mode_{MOVEMENT_STANDBY}
    {

    }

    void HexapodClass::init(bool setting) {
        Servo::init();

    
        calibrationLoad();

        // default to standby mode
        if (!setting)
            processMovement(MOVEMENT_STANDBY);

        Serial.printf("Hexapod init done.\r\n");
    }

    void HexapodClass::processMovement(MovementMode mode, int elapsed) {
        if (mode_ != mode) {
            mode_ = mode;
            movement_.setMode(mode_);
        }

        auto& location = movement_.next(elapsed);
        for(int i=0;i<6;i++) {
            legs_[i].moveTip(location.get(i));
			//Serial.printf("i is %d \r\n",i);
        }
    }

    void HexapodClass::calibrationSave() {

        short data[6*3*2];
		int addr = 0;
        for(int i=0;i<6;i++) {
            for(int j=0;j<3;j++) {
                int offset, scale;
                legs_[i].get(j)->getParameter(offset, scale);
                data[2*3*i + 2*j] = (short)offset;
                data[2*3*i + 2*j + 1] = (short)scale;
            }
        }
Flash_write((const uint8_t *)data,sizeof(data)*2);

    }

    void HexapodClass::calibrationGet(int legIndex, int partIndex, int& offset, int& scale) {
        legs_[legIndex].get(partIndex)->getParameter(offset, scale);
    }

    void HexapodClass::calibrationSet(int legIndex, int partIndex, int offset, int scale) {
        legs_[legIndex].get(partIndex)->setParameter(offset, scale);
    }

    void HexapodClass::calibrationTest(int legIndex, int partIndex, float angle) {
        legs_[legIndex].get(partIndex)->setAngle(angle);
    }

    void HexapodClass::calibrationLoad() {
        short data[6*3*2] = {0};
        uint8_t size = sizeof(data);
        Flash_read((uint8_t *)data,size);

        for(int i=0;i<6;i++) {
            for(int j=0;j<3;j++) {
                legs_[i].get(j)->setParameter(data[2*3*i + 2*j], data[2*3*i + 2*j + 1], false);
            }
        }
    }

}
